#include <iostream>
#include <string>
#include <fstream>
#include <thread>
#include <deque>
#include <sstream>
#include <vector>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_datatypes.h>

#include <yaml-cpp/yaml.h>
#include <map>
#include <pcl/io/pcd_io.h>          // I/O操作头文件
#include <pcl/point_types.h>        // 点类型定义头文件
#include <pcl/registration/icp.h>   // ICP配准类相关头文件

#include <pcl/registration/gicp.h>
#include <pcl/registration/ndt.h>
#include <pcl/features/normal_3d.h> 
#include <pcl/registration/transformation_estimation_point_to_plane.h>


#include <Eigen/Dense>  
#include <Eigen/Geometry>
#include <Eigen/src/Core/Matrix.h>


/*********************************************************
 * @brief 数据对齐操作
 * 1.映射相关点到XY平面；
 * 2.利用ICP函数将相关点进行旋转变换处理；
 * 3.将对齐后的点重新映射Z值；
 * 
 * 使用方法：
 * 1.将相关的GPS TUM格式轨迹数据和对应的SLAM TUM格式轨迹数据存储到对应的文件夹
 *      文件夹的位置配置项在 config/alignICPconfig.yaml 中
 *  GPS文件名: Groud-truth.txt
 *  LLIO算法: LLIO.txt 
 *  LegOdom: LegOdom.txt 
 *  LIOSAM算法: LIOSAM.txt 
 *  FastLio2算法: FastLio2.txt 
 *  FastLioSAM算法: FastLioSAM.txt
 * 
 * 2.启动相关节点，进行轨迹对齐输出
 *   rosrun TopicsTest alignPath2GNSS
 * 
 * 3.对齐后的数据文件为，原本数据文件名 +"_XYZ.txt" 
 *  例如，LLIO.txt 对齐后的TUM格式数据文件为 LLIO_XYZ.txt
 * 
 * 4.更新config读取两个文件直接进行ICP匹配 alignICPconfig.yaml
 *  例如，更改config文件中的 true_Path 和 SLAM_Path 的参数信息
 *********************************************************/

class alignPath2GNSS
{
public:
    std::string root_dir = ROOT_DIR;
    std::string datafile_dir;
    std::string files_dir;

    std::string true_Path; 
    std::string true_Path_Data;
    std::string true_Path_Data_XY;
    std::string true_Path_Data_XY_align;
    std::string true_Path_Data_XYZ;

    std::string SLAM_Path;
    std::string SLAM_Path_Data; 
    std::string SLAM_Path_Data_XY;
    std::string SLAM_Path_Data_XY_align;
    std::string SLAM_Path_Data_XYZ;

    std::deque<std::vector<double>> truePath_Que;
    std::deque<std::vector<double>> truePath_Que_XY;
    std::deque<std::vector<double>> SLAMPath_Que;
    std::deque<std::vector<double>> SLAMPath_Que_XY;
    




    std::string GPSData;  
    std::string LLIOData;
    std::string FastLio2Data;
    std::string LIOSAMData;
    std::string FastLioSAMData;
    std::string LegOdomData;
    std::string GPSData_XY; 
    std::string LLIOData_XY;
    std::string FastLio2Data_XY;
    std::string LIOSAMData_XY;
    std::string FastLioSAMData_XY;
    std::string LegOdomData_XY;

    std::string LLIOData_XY_align;
    std::string FastLio2Data_XY_align;
    std::string LIOSAMData_XY_align;
    std::string FastLioSAMData_XY_align;
    std::string LegOdomData_XY_align;
    std::string GPSData_XYZ;  
    std::string LLIOData_XYZ;
    std::string FastLio2Data_XYZ;
    std::string LIOSAMData_XYZ;
    std::string FastLioSAMData_XYZ;
    std::string LegOdomData_XYZ;

    std::deque<std::vector<double>> GPS_Que;
    std::deque<std::vector<double>> LLIO_Que;
    std::deque<std::vector<double>> FastLio2_Que;
    std::deque<std::vector<double>> LIOSAM_Que;
    std::deque<std::vector<double>> FastLioSAM_Que;
    std::deque<std::vector<double>> LegOdom_Que;

    std::deque<std::vector<double>> GPS_Que_XY;
    std::deque<std::vector<double>> LLIO_Que_XY;
    std::deque<std::vector<double>> FastLio2_Que_XY;
    std::deque<std::vector<double>> LIOSAM_Que_XY;
    std::deque<std::vector<double>> FastLioSAM_Que_XY;
    std::deque<std::vector<double>> LegOdom_Que_XY;

    
    ros::NodeHandle nh;

    alignPath2GNSS()
    {
        YAML::Node yaml;
        yaml = YAML::LoadFile(std::string(std::string(ROOT_DIR)) + "config/alignICPconfig.yaml");
        datafile_dir = yaml["datafile_dir"].as<std::string>();  // 查询config的文件夹位置
        true_Path    = yaml["true_Path"].as<std::string>();     // 查询config的轨迹真值文件
        SLAM_Path    = yaml["SLAM_Path"].as<std::string>();     // 查询config的SLAM轨迹文件
        files_dir    = std::string(std::string(ROOT_DIR)) + datafile_dir;   // 记录文件夹位置

        // 真实轨迹的相关数据文件
        true_Path_Data          = files_dir + true_Path + ".txt";
        true_Path_Data_XY       = files_dir + true_Path + "_XY.txt";
        true_Path_Data_XY_align = files_dir + true_Path + "_XY_align.txt";
        true_Path_Data_XYZ      = files_dir + true_Path + "_XYZ.txt";
        
        // SLAM轨迹的相关数据文件
        SLAM_Path_Data          = files_dir + SLAM_Path + ".txt";
        SLAM_Path_Data_XY       = files_dir + SLAM_Path + "_XY.txt";
        SLAM_Path_Data_XY_align = files_dir + SLAM_Path + "_XY_align.txt";
        SLAM_Path_Data_XYZ      = files_dir + SLAM_Path + "_XYZ.txt";





        GPSData        = files_dir + "Groud-truth.txt";  
        LLIOData       = files_dir + "LLIO.txt";
        FastLio2Data   = files_dir + "FastLio2.txt";
        LIOSAMData     = files_dir + "LIOSAM.txt";
        FastLioSAMData = files_dir + "FastLioSAM.txt";
        LegOdomData    = files_dir + "LegOdom.txt";
        GPSData_XY        = files_dir + "Groud-truth_XY.txt";  
        LLIOData_XY       = files_dir + "LLIO_XY.txt";
        FastLio2Data_XY   = files_dir + "FastLio2_XY.txt";
        LIOSAMData_XY     = files_dir + "LIOSAM_XY.txt";
        FastLioSAMData_XY = files_dir + "FastLioSAM_XY.txt";
        LegOdomData_XY    = files_dir + "LegOdom_XY.txt";

        LLIOData_XY_align       = files_dir + "LLIO_XY_align.txt";
        FastLio2Data_XY_align   = files_dir + "FastLio2_XY_align.txt";
        LIOSAMData_XY_align     = files_dir + "LIOSAM_XY_align.txt";
        FastLioSAMData_XY_align = files_dir + "FastLioSAM_XY_align.txt";
        LegOdomData_XY_align    = files_dir + "LegOdom_XY_align.txt";
        GPSData_XYZ        = files_dir + "Groud-truth_XYZ.txt";  
        LLIOData_XYZ       = files_dir + "LLIO_XYZ.txt";
        FastLio2Data_XYZ   = files_dir + "FastLio2_XYZ.txt";
        LIOSAMData_XYZ     = files_dir + "LIOSAM_XYZ.txt";
        FastLioSAMData_XYZ = files_dir + "FastLioSAM_XYZ.txt";
        LegOdomData_XYZ    = files_dir + "LegOdom_XYZ.txt";
    }

    void readPathData(std::string fileName,  std::deque<std::vector<double>> &data_Que)
    {
        std::string line;
        std::ifstream Files(fileName);

        if ( Files.is_open() )
        {
            while (getline(Files, line))
            {
                std::vector<double> nums;

                const char* split = " ";
                char *s_input = (char*)line.c_str();
                char* p = strtok(s_input, split);   // 获取第一个字符串，分割符号"split"

                double a;
                while (p != NULL)
                {
                    a = atof(p);                // 将字符串转换为double类型，类似还有atoi()--int; atol()--long int;
                    nums.push_back(a);          // 将转换后的数值存入nums容器中
                    p = strtok(NULL, split);    // 继续将余下字符串重新存入
                }

                std::vector<double> buff;
                buff.push_back(nums[0]);
                buff.push_back(nums[1]);
                buff.push_back(nums[2]);
                buff.push_back(nums[3]);
                buff.push_back(nums[4]);
                buff.push_back(nums[5]);
                buff.push_back(nums[6]);
                buff.push_back(nums[7]);

                data_Que.push_back(buff);
            }
            Files.close();
        }
        else
        {
            std::cout << "\033[1;31m Unable to open file.\033[0m" << std::endl;   
        }
    }

    void writeTUMPath_XY(std::string fileName, std::deque<std::vector<double>> data_Que)
    {
        std::ofstream file;
        // file.open(fileName, std::ios::app);
        file.open(fileName);

        if (!file.is_open())
        {
            std::cout << "\033[1;31m Unable to open file.\033[0m" << std::endl;
            return;
        }

        for (int i = 0; i < data_Que.size(); i++)
        {
            std::vector<double> temp;
            temp = data_Que[i];
            file << std::fixed << temp[0]
                    << " " << temp[1]
                    << " " << temp[2]
                    << " " << 0
                    << " " << 0
                    << " " << 0
                    << " " << 0
                    << " " << 0
                    << std::endl;
        }
        file.close();
    }


    void writeTUM_XYZ(std::string fileName, std::deque<std::vector<double>> XY_Que, std::deque<std::vector<double>> data_Que)
    {
        std::ofstream file;
        file.open(fileName);

        if (!file.is_open())
        {
            std::cout << "\033[1;31m Unable to open file.\033[0m" << std::endl;
            return;
        }

        for (int i = 0; i < data_Que.size(); i++)
        {
            std::vector<double> tempXY;
            std::vector<double> tempZ;

            tempXY = XY_Que[i];
            tempZ = data_Que[i];

            static double originX = tempXY[1];
            static double originY = tempXY[2];
            static double originZ = tempZ[3];

            file << std::fixed << tempXY[0]
                    << " " << tempXY[1] - originX
                    << " " << tempXY[2] - originY
                    << " " << tempZ[3]  - originZ
                    << " " << 0
                    << " " << 0
                    << " " << 0
                    << " " << 0
                    << std::endl;
        }
        file.close();

    }
    

    void processPathData2XY()
    {
        readPathData(true_Path_Data , truePath_Que);
        writeTUMPath_XY(true_Path_Data_XY, truePath_Que);

        readPathData(SLAM_Path_Data , SLAMPath_Que);
        writeTUMPath_XY(SLAM_Path_Data_XY, SLAMPath_Que);
    }

    void processSaveICPData2XYZ()
    {
        readPathData(SLAM_Path_Data_XY_align, SLAMPath_Que_XY);
        writeTUM_XYZ(SLAM_Path_Data_XYZ, SLAMPath_Que_XY, SLAMPath_Que);
    }
    
    
    
    // 将点映射到XY平面
    void processPointData2XY(std::string fileName)
    {   
        if (fileName == "GPS")
        {
            readPathData(GPSData, GPS_Que);
            writeTUMPath_XY(GPSData_XY, GPS_Que);
        }
        else if (fileName == "LLIO")
        {
            readPathData(LLIOData, LLIO_Que);
            writeTUMPath_XY(LLIOData_XY, LLIO_Que);
        }
        else if (fileName == "LIOSAM")
        {
            readPathData(LIOSAMData, LIOSAM_Que);
            writeTUMPath_XY(LIOSAMData_XY, LIOSAM_Que);
        }
        else if (fileName == "LegOdom")
        {
            readPathData(LegOdomData, LegOdom_Que);
            writeTUMPath_XY(LegOdomData_XY, LegOdom_Que);
        }
        else if (fileName == "FastLio2")
        {
            readPathData(FastLio2Data, FastLio2_Que);
            writeTUMPath_XY(FastLio2Data_XY, FastLio2_Que);
        }
        else if (fileName == "FastLioSAM")
        {
            readPathData(FastLioSAMData, FastLioSAM_Que);
            writeTUMPath_XY(FastLioSAMData_XY, FastLioSAM_Que);
        }
    }

    // 将对齐后的XY点，加上Z值，完成XYZ
    void processData2XYZ(std::string fileName)
    {
        if (fileName == "LLIO")
        {
            // readPathData(LLIOData, LLIO_Que);
            readPathData(LLIOData_XY_align, LLIO_Que_XY);
            writeTUM_XYZ(LLIOData_XYZ, LLIO_Que_XY, LLIO_Que);
        }
        else if (fileName == "LIOSAM")
        {
            // readPathData(LIOSAMData, LIOSAM_Que);
            readPathData(LIOSAMData_XY_align, LIOSAM_Que_XY);
            writeTUM_XYZ(LIOSAMData_XYZ, LIOSAM_Que_XY, LIOSAM_Que);
        }
        else if (fileName == "LegOdom")
        {
            // readPathData(LegOdomData, LegOdom_Que);
            readPathData(LegOdomData_XY_align, LegOdom_Que_XY);
            writeTUM_XYZ(LegOdomData_XYZ, LegOdom_Que_XY, LegOdom_Que);
        }
        else if (fileName == "FastLio2")
        {
            // readPathData(FastLio2Data, FastLio2_Que);
            readPathData(FastLio2Data_XY_align, FastLio2_Que_XY);
            writeTUM_XYZ(FastLio2Data_XYZ, FastLio2_Que_XY, FastLio2_Que);
        }
        else if (fileName == "FastLioSAM")
        {
            // readPathData(FastLioSAMData, FastLioSAM_Que);
            readPathData(FastLioSAMData_XY_align, FastLioSAM_Que_XY);
            writeTUM_XYZ(FastLioSAMData_XYZ, FastLioSAM_Que_XY, FastLioSAM_Que);
        }       
    }




    // void processTotalAlign(std::string SLAM_Name)
    // {
    //     processPointData2XY("GPS");
    //     processPointData2XY(SLAM_Name);
    // }

    
    
    void deskewData()
    {
        double timeFrom, timeTo;
        double ratioFront, ratioBack;
        std::vector<double> dataFrom, dataTo;

        readPathData(GPSData, GPS_Que);
        readPathData(LLIOData, LLIO_Que);
        readPathData(LIOSAMData, LIOSAM_Que);

        for (int i = 0; i < 15; i++)
        {
            std::vector<double> dataGPS = GPS_Que.front();
            std::deque<double> dataCurAlign;
            // dataCurAlign[0] = dataGPS[0];
            
            if (sync_dataTime(LLIO_Que))
            {
                align_dataTime_FromTo(LLIO_Que, GPS_Que, dataFrom, dataTo, timeFrom, timeTo);
                calculateRatio(GPS_Que, ratioFront, ratioBack, timeFrom, timeTo);
                align_dataPoint(dataCurAlign, dataGPS, dataFrom, dataTo, ratioFront);

                // std::cout << std::fixed << timeFrom << std::endl;
                // std::cout << std::fixed << timeTo << std::endl;
                // std::cout << std::fixed << ratioFront << std::endl;
                // std::cout << std::fixed << dataCurAlign[0] << std::endl;

            }

            GPS_Que.pop_front();
        }
    }

    
    void align_dataPoint(std::deque<double> &dataCur, std::vector<double> dataTarget, std::vector<double> dataFrom, std::vector<double> dataTo, double ratioFront)
    {
        // 四元数球面插值
        tf::Quaternion qFront, qBack, qCur;
        qFront.setX(dataFrom[4]);
        qFront.setY(dataFrom[5]);
        qFront.setZ(dataFrom[6]);
        qFront.setW(dataFrom[7]);
        qBack.setX(dataTo[4]);
        qBack.setY(dataTo[5]);
        qBack.setZ(dataTo[6]);
        qBack.setW(dataTo[7]);
        qCur = qFront.slerp(qBack, ratioFront);

        // 位置线性插值
        tf::Point pFront, pBack, pCur;
        pFront.setX(dataFrom[1]);
        pFront.setY(dataFrom[2]);
        pFront.setZ(dataFrom[3]);
        pBack.setX(dataTo[1]);
        pBack.setY(dataTo[2]);
        pBack.setZ(dataTo[3]);
        pCur = pFront.lerp(pBack, ratioFront);

        // 写入数据
        dataCur[0] = dataTarget[0];
        dataCur[1] = pCur.getX();
        dataCur[2] = pCur.getY();
        dataCur[3] = pCur.getZ();
        dataCur[4] = qCur.getX();
        dataCur[5] = qCur.getY();
        dataCur[6] = qCur.getZ();
        dataCur[7] = qCur.getW();
    }
    
    void calculateRatio(std::deque<std::vector<double>> &target_Que, double &ratioFront, double &ratioBack, double timeFrom, double timeTo)
    {
        std::vector<double> dataGPS = target_Que.front();

        ratioFront = (dataGPS[0] - timeFrom) / (timeTo - timeFrom); // 前向部分的所占比率
        ratioBack = (timeTo - dataGPS[0]) / (timeTo - timeFrom);    // 后向部分的所占比率
    }
    
    
    void align_dataTime_FromTo(std::deque<std::vector<double>> &data_Que, std::deque<std::vector<double>> &target_Que, std::vector<double> &dataFrom, std::vector<double> &dataTo, double &timeFrom, double &timeTo)
    {
        for (int i = 0; i < (int)data_Que.size(); ++i)
        {
            std::vector<double> dataTemp = data_Que[i];
            std::vector<double> dataGPS = target_Que.front();

            if (dataTemp[0] < dataGPS[0])
            {
                timeFrom = dataTemp[0];
                dataFrom = dataTemp;
                continue;
            }
            else
            {
                timeTo = dataTemp[0];
                dataTo = dataTemp;
                break;
            }
        }
    }
    
    
    bool sync_dataTime(std::deque<std::vector<double>> &data_Que)
    {
        if (GPS_Que.empty())
            return false;
        
        std::vector<double> data_Buff;
        std::vector<double> GPS_Buff;       

        while (!GPS_Que.empty())
        {
            data_Buff = data_Que.front();
            GPS_Buff = GPS_Que.front();

            if (data_Buff[0] < GPS_Buff[0] - 0.4)
                data_Que.pop_front();
            else
                break;
        }
        return true;
    }


private:

    
};






class ICP2GNSSPath
{
public:
    std::string root_dir = ROOT_DIR;
    // std::string datafile_dir = "pathData/sportsGround-04/";
    // const std::string files_dir =  std::string(std::string(ROOT_DIR)) + datafile_dir;
    std::string datafile_dir;
    std::string files_dir;

    std::vector<float> init_params; // ICP初始化时的参数
    std::string icp_pattern;        // 使用的ICP方法
    std::string SLAM_Name;          // 轨迹使用的SLAM算法
    std::string true_Path;
    std::string SLAM_Path;
   
    typedef Eigen::Matrix4f (ICP2GNSSPath::*ICPFunPtr)(pcl::PointCloud<pcl::PointXYZI>::Ptr, pcl::PointCloud<pcl::PointXYZI>::Ptr, Eigen::Matrix4f&); //函数指针
 
    // TUM的格式
    struct TumTrajectoryPoint 
    {  
        double timestamp;  
        Eigen::Vector3d position;  
        Eigen::Quaternionf rotation;  
    };

    ICP2GNSSPath()
    {
        // 使用yaml进行参数读取
        YAML::Node yaml;
        yaml = YAML::LoadFile(std::string(std::string(ROOT_DIR)) + "config/alignICPconfig.yaml");
        init_params = yaml["init_params"].as<std::vector<float>>();     // ICP配准使用的初始参数
        icp_pattern = yaml["icp_pattern"].as<std::string>();            // 使用的ICP配准方法
        SLAM_Name   = yaml["SLAM"].as<std::string>();                   // 数据使用的SLAM算法
        datafile_dir= yaml["datafile_dir"].as<std::string>();           // 数据存放的文件夹（当前工程下）
        true_Path    = yaml["true_Path"].as<std::string>();             // 查询config的轨迹真值文件
        SLAM_Path    = yaml["SLAM_Path"].as<std::string>();             // 查询config的SLAM轨迹文件
        files_dir =  std::string(std::string(ROOT_DIR)) + datafile_dir; // 存放文件夹的完整路径
    }

    //  读取TUM格式的轨迹文件  
    std::vector<TumTrajectoryPoint> readTumTrajectory(const std::string& filename)
    {  
        std::vector<TumTrajectoryPoint> points;  
        std::ifstream file(filename);  
        if (!file.is_open())
        {  
            std::cerr << "Failed to open file: " << filename << std::endl;  
            return points;  
        }  
    
        std::string line;  
        while (std::getline(file, line)) {  
            if (line.empty() || line[0] == '#') continue; // 忽略空行和注释  
    
            std::istringstream iss(line);  
            double timestamp;  
            Eigen::Vector3d position;  
            Eigen::Quaternionf rotation;  
            TumTrajectoryPoint trajPoint;
            // 解析每一行的数据，这里假设你已经知道数据的格式  
            iss >> timestamp >> position(0) >> position(1) >> position(2) >> rotation.x() >> rotation.y() >> rotation.z() >> rotation.w();  
            trajPoint.timestamp = timestamp;
            trajPoint.position = position;
            trajPoint.rotation = rotation;
            points.emplace_back(trajPoint);  
        }  
    
        file.close();  
        return points;
    }


    // 将轨迹点写入TUM格式的文件  
    void writeTUMTrajectory(const std::string& filename, const std::vector<TumTrajectoryPoint>& trajectory) 
    {  
        std::ofstream file(filename);  
        if (!file.is_open()) {  
            std::cerr << "Failed to open file for writing: " << filename << std::endl;  
            return;  
        }  
    
        for (const auto& point : trajectory) {  
            // 输出时间戳、位置和旋转（四元数）
            // 获取里程计的时间戳
            double timestamp = point.timestamp;

            // 获取traj的位置信息
            double x = point.position.x();
            double y = point.position.y();
            double z = point.position.z();

            // 获取traj的姿态信息（四元数表示）
            double qx = point.rotation.x();
            double qy = point.rotation.y();
            double qz = point.rotation.z();
            double qw = point.rotation.w();
            file << std::fixed << std::setprecision(9) << timestamp << " "
                << x << " " << y << " " << z << " "
                << qx << " " << qy << " " << qz << " " << qw << std::endl;
        }  
        file.close();  
    }

    
    // PointToPlane
    Eigen::Matrix4f PointToPlane(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud1,
                             pcl::PointCloud<pcl::PointXYZI>::Ptr cloud2,
                             Eigen::Matrix4f& guess)
    {
    pcl::PointCloud<pcl::PointNormal>::Ptr src(new pcl::PointCloud<pcl::PointNormal>);
    pcl::copyPointCloud(*cloud1, *src);
    pcl::PointCloud<pcl::PointNormal>::Ptr tgt(new pcl::PointCloud<pcl::PointNormal>);
    pcl::copyPointCloud(*cloud2, *tgt);

    pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est;
    norm_est.setSearchMethod(pcl::search::KdTree<pcl::PointNormal>::Ptr(new pcl::search::KdTree<pcl::PointNormal>));
    norm_est.setKSearch(10);
    norm_est.setInputCloud(tgt);
    norm_est.compute(*tgt);

    pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
    typedef pcl::registration::TransformationEstimationPointToPlane<pcl::PointNormal, pcl::PointNormal> PointToPlane;
    boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
    icp.setTransformationEstimation(point_to_plane);  // key
    /*
    typedef pcl::registration::CorrespondenceEstimationNormalShooting<PointNormal,
    PointNormal, PointNormal> CorrEstNS;
    CorrEstNS::Ptr corrEst(new CorrEstNS);
    icp.setCorrespondenceEstimation(corrEst);
    */
    icp.setInputSource(src);
    icp.setInputTarget(tgt);
    // icp.setRANSACOutlierRejectionThreshold(ransac_par);
    icp.setRANSACIterations(20);
    icp.setMaximumIterations(100);
    icp.setTransformationEpsilon(1e-3);
    pcl::PointCloud<pcl::PointNormal> output;
    icp.align(output);  // align 的另一个重载可以设置一个初始矩阵guess
    std::cout << "score: " << icp.getFitnessScore() << std::endl;
    return icp.getFinalTransformation();
    }

    // icpPlaneToPlane
    Eigen::Matrix4f icpPlaneToPlane(pcl::PointCloud<pcl::PointXYZI>::Ptr src, pcl::PointCloud<pcl::PointXYZI>::Ptr tar, Eigen::Matrix4f& guess)
    {
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;  // GICP 泛化的ICP，或者叫Plane to Plane
                                                                                // ICP
    icp.setTransformationEpsilon(0.0000000001);
    icp.setMaxCorrespondenceDistance(2.0);
    icp.setMaximumIterations(50);
    icp.setRANSACIterations(20);
    icp.setInputTarget(tar);
    icp.setInputSource(src);
    pcl::PointCloud<pcl::PointXYZI> unused_result;
    icp.align(unused_result, guess);
    std::cout << "score: " << icp.getFitnessScore();
    return icp.getFinalTransformation();
    }

    void addNormal(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
               pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_with_normals)
    {
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    pcl::search::KdTree<pcl::PointXYZI>::Ptr searchTree(new pcl::search::KdTree<pcl::PointXYZI>);
    searchTree->setInputCloud(cloud);

    pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> normalEstimator;
    normalEstimator.setInputCloud(cloud);
    normalEstimator.setSearchMethod(searchTree);
    normalEstimator.setKSearch(15);
    normalEstimator.compute(*normals);

    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
    }

    Eigen::Matrix4f icpPointToPlane(pcl::PointCloud<pcl::PointXYZI>::Ptr src, pcl::PointCloud<pcl::PointXYZI>::Ptr tar, Eigen::Matrix4f& guess)
    {
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_source_normals(new pcl::PointCloud<pcl::PointXYZINormal>());
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_target_normals(new pcl::PointCloud<pcl::PointXYZINormal>());
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_source_trans_normals(new pcl::PointCloud<pcl::PointXYZINormal>());

    addNormal(tar, cloud_target_normals);
    addNormal(src, cloud_source_normals);

    pcl::IterativeClosestPointWithNormals<pcl::PointXYZINormal, pcl::PointXYZINormal>::Ptr icp(
        new pcl::IterativeClosestPointWithNormals<pcl::PointXYZINormal, pcl::PointXYZINormal>());
    icp->setTransformationEpsilon(0.0000000001);
    icp->setMaxCorrespondenceDistance(2.0);
    icp->setMaximumIterations(50);
    icp->setRANSACIterations(20);
    icp->setInputSource(cloud_source_normals);  //
    icp->setInputTarget(cloud_target_normals);
    icp->align(*cloud_source_trans_normals, guess);  //
    std::cout << "score: " << icp->getFitnessScore() << std::endl;
    return icp->getFinalTransformation();
    }

    //NDT
    Eigen::Matrix4f NDT(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_source,  
                                pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target,  
                                Eigen::Matrix4f& initial_guess)  
    {  
        // 将输入点云转换为带法线的点云  
        pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_normals(new pcl::PointCloud<pcl::PointNormal>);  
        pcl::PointCloud<pcl::PointNormal>::Ptr cloud_target_normals(new pcl::PointCloud<pcl::PointNormal>);  
        pcl::copyPointCloud(*cloud_source, *cloud_source_normals);  
        pcl::copyPointCloud(*cloud_target, *cloud_target_normals);  
    
        // 计算法线  
        pcl::NormalEstimation<pcl::PointXYZI, pcl::PointNormal> ne;  
        ne.setSearchMethod(pcl::search::KdTree<pcl::PointXYZI>::Ptr(new pcl::search::KdTree<pcl::PointXYZI>()));  
        ne.setRadiusSearch(0.03); // 设置搜索半径  
        ne.setInputCloud(cloud_source);  
        pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_normals_computed(new pcl::PointCloud<pcl::PointNormal>);  
        ne.compute(*cloud_source_normals_computed);  
        *cloud_source_normals = *cloud_source_normals_computed;  
    
        ne.setInputCloud(cloud_target);  
        pcl::PointCloud<pcl::PointNormal>::Ptr cloud_target_normals_computed(new pcl::PointCloud<pcl::PointNormal>);  
        ne.compute(*cloud_target_normals_computed);  
        *cloud_target_normals = *cloud_target_normals_computed;  
    
        // 初始化NDT对象  
        pcl::NormalDistributionsTransform<pcl::PointNormal, pcl::PointNormal> ndt;  
        ndt.setInputSource(cloud_source_normals);  
        ndt.setInputTarget(cloud_target_normals);  
        ndt.setMaxCorrespondenceDistance(150); // 设置最大对应距离  
        ndt.setTransformationEpsilon(1e-3); // 设置收敛阈值  
        ndt.setStepSize(0.1); // 设置步长  
        ndt.setMaximumIterations(100); // 设置线程数  
        ndt.setEuclideanFitnessEpsilon(0.01);

        // 执行配准  
        pcl::PointCloud<pcl::PointNormal> aligned_cloud;  
        ndt.align(aligned_cloud);  

        // 输出配准结果  
        std::cout << "score: " << ndt.getFitnessScore() << std::endl;  
        std::cout << "has converged:" << ndt.hasConverged() << std::endl;  
    
        return ndt.getFinalTransformation();
    }



    
    
    
    // PointToPoint 点对点映射ICP，效果最好
    Eigen::Matrix4f PointToPoint(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_source, 
                                    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target, 
                                                            Eigen::Matrix4f& guess)
    {
        // ICP
        pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
        // pcl::IterativeClosestPointWithNormals<pcl::PointXYZI, pcl::PointXYZI> icp;
        pcl::search::KdTree<pcl::PointXYZI>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZI>);
        tree1->setInputCloud(cloud_source);
        pcl::search::KdTree<pcl::PointXYZI>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZI>);
        tree2->setInputCloud(cloud_target);
        icp.setSearchMethodSource(tree1);
        icp.setSearchMethodTarget(tree2);
        icp.setInputSource(cloud_source);
        icp.setInputTarget(cloud_target);
        icp.setMaxCorrespondenceDistance(150);  // 设置两点间的最大匹配距离 150        （最大欧式距离差值）
        icp.setTransformationEpsilon(1e-3);     // 设置两次迭代间变化的收敛阈值 0.001  （最大转换矩阵差值）
        icp.setEuclideanFitnessEpsilon(0.01);   // 设置两次迭代的全局误差收敛值 0.01   （均方误差和）
        icp.setMaximumIterations(100);          // 设置最大迭代次数 100
        pcl::PointCloud<pcl::PointXYZI> output;
        icp.align(output);

        // 打印配准相关输入信息
        std::cout << "has converged:" << icp.hasConverged() << std::endl;
        std::cout << "score: " << icp.getFitnessScore() << std::endl;
        std::cout << "Transformation: "<< "\n" << icp.getFinalTransformation() << std::endl;
        return icp.getFinalTransformation();
    }


    Eigen::Matrix4f genTransformation(Eigen::Vector3f& r, Eigen::Vector3f& t)
    {
        Eigen::AngleAxisf init_rotation_x(r.x(), Eigen::Vector3f::UnitX());
        Eigen::AngleAxisf init_rotation_y(r.y(), Eigen::Vector3f::UnitY());
        Eigen::AngleAxisf init_rotation_z(r.z(), Eigen::Vector3f::UnitZ());
        Eigen::Translation3f init_translation(t.x(), t.y(), t.z());
        return (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix();
    }

    void displayAngel(Eigen::Matrix4f& transformation)
    {
        double rx, ry, rz, tx, ty, tz;
        ry = (180 / M_PI) * atan(-(transformation(2, 0) / sqrt(pow(transformation(0, 0), 2) + pow(transformation(1, 0), 2))));
        rz = (180 / M_PI) * atan((transformation(1, 0) / transformation(0, 0)));
        rx = (180 / M_PI) * atan((transformation(2, 1) / transformation(2, 2)));
        tx = transformation(0, 3);
        ty = transformation(1, 3);
        tz = transformation(2, 3);
        // std::cout << "-------------------" << std::endl;
        // std::cout << "rx = " << rx << std::endl;
        // std::cout << "ry = " << ry << std::endl;
        // std::cout << "rz = " << rz << std::endl;
        // std::cout << "tx = " << tx << std::endl;
        // std::cout << "ty = " << ty << std::endl;
        // std::cout << "tz = " << tz << std::endl;

        std::cout << std::fixed << "\033[0;34m" << "rx = " << rx << "\033[0m" << std::endl;
        std::cout << std::fixed << "\033[0;34m" << "ry = " << ry << "\033[0m" << std::endl;
        std::cout << std::fixed << "\033[0;34m" << "rz = " << rz << "\033[0m" << std::endl;

        std::cout << std::fixed << "\033[0;33m" << "tx = " << tx << "\033[0m" << std::endl;
        std::cout << std::fixed << "\033[0;33m" << "ty = " << ty << "\033[0m" << std::endl;
        std::cout << std::fixed << "\033[0;33m" << "tz = " << tz << "\033[0m" << std::endl;

        // std::cout << "-------------------" << std::endl;
        // std::cout << rx << '\n' << ry << '\n' << rz << '\n' << tx << '\n' << ty << '\n' << tz << std::endl;
    }

    Eigen::Matrix4f run(pcl::PointCloud<pcl::PointXYZI>::Ptr src, 
                                pcl::PointCloud<pcl::PointXYZI>::Ptr tar, 
                                            const std::vector<float>& params)
    {
        Eigen::Vector3f r(params[0], params[1], params[2]);
        Eigen::Vector3f t(params[3], params[4], params[5]);
        // Eigen::Matrix4f guess = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f guess = genTransformation(r, t);
        return PointToPoint(src, tar, guess);
    }


    Eigen::Matrix4f
    run_useMap(pcl::PointCloud<pcl::PointXYZI>::Ptr src, pcl::PointCloud<pcl::PointXYZI>::Ptr tar, const std::vector<float>& params, ICPFunPtr icp)
    {
        Eigen::Vector3f r(params[0], params[1], params[2]);
        Eigen::Vector3f t(params[3], params[4], params[5]);
        // Eigen::Matrix4f guess = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f guess = genTransformation(r, t);

        // Eigen::Matrix4f result = (this->*icp_map["PointToPoint"])(src, tar, guess);
        return (this->*icp)(src, tar, guess);
    }





    void initialzeOutputFile(std::vector<TumTrajectoryPoint>& finalTrajectory,
                                const std::vector<TumTrajectoryPoint> slamTrajectory,
                                    const std::vector<TumTrajectoryPoint> gtTrajectory)
    {
        //去小的size给finalTrajectory，以防读取gtTrajectory和slamTrajectory轨迹时候容器越界
        // int size = std::min(gtTrajectory.size(), slamTrajectory.size());
        // int size = std::max(gtTrajectory.size(), slamTrajectory.size());
        int size = slamTrajectory.size();

        for (int i = 0; i < size; i++)
        {
            // finalTrajectory[i].timestamp = gtTrajectory[i].timestamp;//和groudTruth轨迹的时间戳对齐，解决evo_ape时间戳不对齐对的问题        
            finalTrajectory[i].timestamp = slamTrajectory[i].timestamp;
            finalTrajectory[i].rotation = slamTrajectory[i].rotation;//用原来的旋转初始化
        }
    }


    void processICP2GNSS(std::string fileName)
    {
        const std::string slamTrajectoryFile   = files_dir + fileName + "_XY.txt";
        const std::string gtTrajectoryFile     = files_dir + "Groud-truth_XY.txt";
        const std::string outputTrajectoryFile = files_dir + fileName + "_XY_align.txt";

        // std::vector<float> init_params = {0, 0, 0, 0, 0, 0};
        // std::string icp_pattern = "PointToPoint";

        //创建四个pcl::PointCloud<pcl::PointXYZI>共享指针，并初始化它们
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_source (new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target (new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target_transform(new pcl::PointCloud<pcl::PointXYZI>());
        pcl::PointCloud<pcl::PointXYZI>::Ptr Final(new pcl::PointCloud<pcl::PointXYZI>);

        // 读取SLAM轨迹和真值轨迹 必须要初始化，否则会coredump
        std::vector<ICP2GNSSPath::TumTrajectoryPoint> slamTrajectory = readTumTrajectory(slamTrajectoryFile);  
        std::vector<ICP2GNSSPath::TumTrajectoryPoint> gtTrajectory = readTumTrajectory(gtTrajectoryFile);  
        std::vector<ICP2GNSSPath::TumTrajectoryPoint> finalTrajectory;
        // int max_size = std::max(gtTrajectory.size(), slamTrajectory.size());//取大的轨迹来resize finalTrajectory
        // finalTrajectory.resize(max_size);
        int slam_size =  slamTrajectory.size();
        finalTrajectory.resize(slam_size);

        //需要初始化slamTrajectory，否则会coredumped(指针泄露)
        cloud_source->resize(slamTrajectory.size());  
        cloud_target->resize(slamTrajectory.size());  
        cloud_target_transform->resize(slamTrajectory.size());  
        Final->resize(slamTrajectory.size());
        initialzeOutputFile(finalTrajectory, slamTrajectory, gtTrajectory);//把slamTraj中的非position部分加入到finalTraj中

        // SLAM的轨迹点，存储为点云
        for (int i = 0; i < slamTrajectory.size(); i++)
        {
            cloud_source->points[i].x = slamTrajectory[i].position[0];//coredumped?
            cloud_source->points[i].y = slamTrajectory[i].position[1];
            cloud_source->points[i].z = slamTrajectory[i].position[2];
        }

        // GPS的轨迹点，存储为点云
        for (int i = 0; i < gtTrajectory.size(); i++)
        {
            cloud_target->points[i].x = gtTrajectory[i].position[0];
            cloud_target->points[i].y = gtTrajectory[i].position[1];
            cloud_target->points[i].z = gtTrajectory[i].position[2];
        }

        Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();

        // 使用PCL的ICP匹配进行轨迹的旋转变换，run()直接使用ICP， run_useMap()使用std::map进行选择ICP算法
        // Eigen::Matrix4f transformation = run(cloud_source, cloud_target, init_params);
        Eigen::Matrix4f transformation = run_useMap(cloud_source, cloud_target, init_params, icp_map[icp_pattern]);


        displayAngel(transformation);
        
        // 从cloud_source到cloud_target_transform的变换T
        pcl::transformPointCloud(*cloud_source, *cloud_target_transform, transformation);

        // 遍历转换后点云中的每个点 s,并将转换后点的position和rotation进行更新
        for (size_t i = 0; i < cloud_target_transform->size(); ++i) {  
            // 获取当前点的坐标  
            const pcl::PointXYZI& point = cloud_target_transform->points[i];  
                
            // 将坐标复制到finalTrajectory中对应点的position属性  
            finalTrajectory[i].position(0) = point.x;  
            finalTrajectory[i].position(1) = point.y;  
            finalTrajectory[i].position(2) = point.z; 
            // T_W_B0 = T_W_B0 * (T_B0_B01).inv: 计算transform_target到W系下的变换
            Eigen::Matrix3f transformation_R = transformation.block<3,3>(0, 0);
            Eigen::Quaternionf transformation_q(transformation_R);
            finalTrajectory[i].rotation = finalTrajectory[i].rotation * transformation_q.inverse();
        }
        
        writeTUMTrajectory(outputTrajectoryFile, finalTrajectory);  
        std::cout << "Trajectory written to " << outputTrajectoryFile << std::endl;
    }


    void processICP2truePath()
    {
        const std::string slamTrajectoryFile   = files_dir + SLAM_Path + "_XY.txt";
        const std::string gtTrajectoryFile     = files_dir + true_Path + "_XY.txt";
        const std::string outputTrajectoryFile = files_dir + SLAM_Path + "_XY_align.txt";

        // std::vector<float> init_params = {0, 0, 0, 0, 0, 0};
        // std::string icp_pattern = "PointToPoint";

        //创建四个pcl::PointCloud<pcl::PointXYZI>共享指针，并初始化它们
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_source (new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target (new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target_transform(new pcl::PointCloud<pcl::PointXYZI>());
        pcl::PointCloud<pcl::PointXYZI>::Ptr Final(new pcl::PointCloud<pcl::PointXYZI>);

        // 读取SLAM轨迹和真值轨迹 必须要初始化，否则会coredump
        std::vector<ICP2GNSSPath::TumTrajectoryPoint> slamTrajectory = readTumTrajectory(slamTrajectoryFile);  
        std::vector<ICP2GNSSPath::TumTrajectoryPoint> gtTrajectory = readTumTrajectory(gtTrajectoryFile);  
        std::vector<ICP2GNSSPath::TumTrajectoryPoint> finalTrajectory;
        // int max_size = std::max(gtTrajectory.size(), slamTrajectory.size());//取大的轨迹来resize finalTrajectory
        // finalTrajectory.resize(max_size);
        int slam_size =  slamTrajectory.size();
        finalTrajectory.resize(slam_size);

        //需要初始化slamTrajectory，否则会coredumped(指针泄露)
        cloud_source->resize(slamTrajectory.size());  
        cloud_target->resize(gtTrajectory.size());  
        cloud_target_transform->resize(slamTrajectory.size());  
        Final->resize(slamTrajectory.size());
        initialzeOutputFile(finalTrajectory, slamTrajectory, gtTrajectory);//把slamTraj中的非position部分加入到finalTraj中

        // SLAM的轨迹点，存储为点云
        for (int i = 0; i < slamTrajectory.size(); i++)
        {
            cloud_source->points[i].x = slamTrajectory[i].position[0];//coredumped?
            cloud_source->points[i].y = slamTrajectory[i].position[1];
            cloud_source->points[i].z = slamTrajectory[i].position[2];
        }

        // GPS的轨迹点，存储为点云
        for (int i = 0; i < gtTrajectory.size(); i++)
        {
            cloud_target->points[i].x = gtTrajectory[i].position[0];
            cloud_target->points[i].y = gtTrajectory[i].position[1];
            cloud_target->points[i].z = gtTrajectory[i].position[2];
        }

        Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();

        // 使用PCL的ICP匹配进行轨迹的旋转变换，run()直接使用ICP， run_useMap()使用std::map进行选择ICP算法
        // Eigen::Matrix4f transformation = run(cloud_source, cloud_target, init_params);
        Eigen::Matrix4f transformation = run_useMap(cloud_source, cloud_target, init_params, icp_map[icp_pattern]);


        displayAngel(transformation);
        
        // 从cloud_source到cloud_target_transform的变换T
        pcl::transformPointCloud(*cloud_source, *cloud_target_transform, transformation);

        // 遍历转换后点云中的每个点 s,并将转换后点的position和rotation进行更新
        for (size_t i = 0; i < cloud_target_transform->size(); ++i) {  
            // 获取当前点的坐标  
            const pcl::PointXYZI& point = cloud_target_transform->points[i];  
                
            // 将坐标复制到finalTrajectory中对应点的position属性  
            finalTrajectory[i].position(0) = point.x;  
            finalTrajectory[i].position(1) = point.y;  
            finalTrajectory[i].position(2) = point.z; 
            // T_W_B0 = T_W_B0 * (T_B0_B01).inv: 计算transform_target到W系下的变换
            Eigen::Matrix3f transformation_R = transformation.block<3,3>(0, 0);
            Eigen::Quaternionf transformation_q(transformation_R);
            finalTrajectory[i].rotation = transformation_q * finalTrajectory[i].rotation;
        }
        
        writeTUMTrajectory(outputTrajectoryFile, finalTrajectory);  
        std::cout << "Trajectory written to " << outputTrajectoryFile << std::endl;



        /*************************************************/
        
        const std::string SLAM_TrajectoryFile  = files_dir + SLAM_Path + ".txt";
        const std::string align_XYZ_TrajectoryFile = files_dir + SLAM_Path + "_align.txt";

        pcl::PointCloud<pcl::PointXYZI>::Ptr XYZ_cloud_source (new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr XYZ_cloud_target_transform(new pcl::PointCloud<pcl::PointXYZI>());
        pcl::PointCloud<pcl::PointXYZI>::Ptr SLAM_Final(new pcl::PointCloud<pcl::PointXYZI>);

        std::vector<ICP2GNSSPath::TumTrajectoryPoint> SLAM_Trajectory = readTumTrajectory(SLAM_TrajectoryFile);
        std::vector<ICP2GNSSPath::TumTrajectoryPoint> SLAM_finalTrajectory;

        int SLAM_size =  SLAM_Trajectory.size();
        SLAM_finalTrajectory.resize(SLAM_size);

        XYZ_cloud_source->resize(SLAM_Trajectory.size()); 
        XYZ_cloud_target_transform->resize(SLAM_Trajectory.size()); 
        SLAM_Final->resize(SLAM_Trajectory.size()); 

        initialzeOutputFile(SLAM_finalTrajectory, SLAM_Trajectory, gtTrajectory);

        for (int i = 0; i < SLAM_Trajectory.size(); i++)
        {
            XYZ_cloud_source->points[i].x = SLAM_Trajectory[i].position[0];//coredumped?
            XYZ_cloud_source->points[i].y = SLAM_Trajectory[i].position[1];
            XYZ_cloud_source->points[i].z = SLAM_Trajectory[i].position[2];
        }

        pcl::transformPointCloud(*XYZ_cloud_source, *XYZ_cloud_target_transform, transformation);

        double tx, ty, tz;
        tx = transformation(0, 3);
        ty = transformation(1, 3);
        tz = transformation(2, 3);
        
        for (size_t i = 0; i < XYZ_cloud_target_transform->size(); ++i) {  
            // 获取当前点的坐标  
            const pcl::PointXYZI& point = XYZ_cloud_target_transform->points[i];  
                
            // 将坐标复制到finalTrajectory中对应点的position属性  
            SLAM_finalTrajectory[i].position(0) = point.x - tx;  
            SLAM_finalTrajectory[i].position(1) = point.y - ty;  
            SLAM_finalTrajectory[i].position(2) = point.z - tz; 
            // T_W_B0 = T_W_B0 * (T_B0_B01).inv: 计算transform_target到W系下的变换
            Eigen::Matrix3f transformation_R = transformation.block<3,3>(0, 0);
            Eigen::Quaternionf transformation_q(transformation_R);
            SLAM_finalTrajectory[i].rotation = transformation_q * SLAM_finalTrajectory[i].rotation;
        }

        writeTUMTrajectory(align_XYZ_TrajectoryFile, SLAM_finalTrajectory);  
        std::cout << "Trajectory written to " << align_XYZ_TrajectoryFile << std::endl;

        /*************************************************/


    }


    // 该函数比较繁琐，更换其它数据的转换会相对较麻烦
    void processTotalAlign()
    {
        alignPath2GNSS ALIGIN;

        ALIGIN.processPointData2XY("GPS");      // 读取GPS真实值，映射为XY平面坐标
        ALIGIN.processPointData2XY(SLAM_Name);  // 读取SLAM估计值，映射为XY平面坐标
        processICP2GNSS(SLAM_Name);             // 进行平面坐标的轨迹转换对齐
        ALIGIN.processData2XYZ(SLAM_Name);      // 将对齐后的坐标轨迹映射为XYZ坐标
    }

    // 使用这个函数直接更改config的相关参数即可进行快速转换
    void ICP2TruePath()
    {
        alignPath2GNSS ALIGIN;
        ALIGIN.processPathData2XY();        // 读取truePath和SLAMPath的数值，映射为XY平面坐标
        processICP2truePath();              // 将两个映射XY的坐标进行平面坐标的轨迹转换对齐（对齐参考轨迹为truePath的轨迹）
        ALIGIN.processSaveICPData2XYZ();    // 将对齐后的SLAMPath坐标轨迹映射为XYZ坐标
    }


private:

    std::map<std::string, ICPFunPtr> icp_map{{"PointToPoint", &ICP2GNSSPath::PointToPoint},
                                             {"icpPlaneToPlane", &ICP2GNSSPath::icpPlaneToPlane},
                                             {"icpPointToPlane", &ICP2GNSSPath::icpPointToPlane},
                                             {"PointToPlane", &ICP2GNSSPath::PointToPlane},
                                             {"NDT", &ICP2GNSSPath::NDT}};

};



int main(int argc, char** argv)
{
    ros::init(argc, argv, "alignPath2GNSS");

    ICP2GNSSPath ICP;

    ROS_INFO("\033[1;32m----> Align Path Data To GNSS Process is Started.\033[0m");

    // ICP.processTotalAlign();
    ICP.ICP2TruePath();

    return 0;
}